/*
 *  Mikrokopter Flight control Serial Interface
 *  Copyright (C) 2010, CYPHY lab
 *  Inkyu Sa <i.sa@qut.edu.au>
 *
 *  http://wiki.qut.edu.au/display/cyphy
 *
 *
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "Comp_filter.h"

ros::Timer timer;
ros::WallTimer walltimer;

long g_nCounter=0;
long g_nCounter1=0;
long g_nCounter_h=0;
long g_nShapeCnt=0;
unsigned int g_throttle=0;
int g_barome_height=0;
char g_quad_state=ON_GROUND;


#if 0

unsigned char g_buf[300];
unsigned char g_buf_debug[300];
unsigned char g_txd_buffer[TXD_BUFFER_LEN];
unsigned char g_rxd_buffer[RXD_BUFFER_LEN];
//unsigned char txd_complete;
unsigned char g_ReceivedBytes;
unsigned char *g_pRxData;
unsigned char g_RxDataLen;
//double g_dblQuad_x;
//double g_dblQuad_y;
//double g_dblQuad_theta;
double g_pitch_temp;
double g_roll_temp;
double g_pitch_new;
double g_pitch_cmd;
double g_roll_cmd;
double g_roll_new;
double g_yaw_new;
double g_yaw_temp;
double dblDistance;

struct str_Data3D g_Data3D,g_Data3D_Temp;
DebugOut_t g_DebugData,g_DebugData_Temp;
Position_t g_PositionFromAcc,g_PostPosition,g_EstimatePosition,g_CurPosition,g_GoalPosition,g_GoalVel,g_errorPos;
Control_t g_StickControl,g_PostStickControl;
#endif
extern unsigned char thrust[TABLE_SIZE];
Position_t g_CurPosition,g_PostPosition,g_GoalPosition;
Velocity_t g_W_Vel,g_B_Vel,g_B_ACC,g_B_esti_Vel,g_B_predic_esti_Vel,g_B_goal_Vel,g_B_goal_Vel_;

#if 0
void callback(cyphy_CF::onlinegainConfig &config, uint32_t level)
	{
	  
	}
#endif


int main(int argc, char **argv)
{
   int interval=0;
   int nLength;
   int cnt=3;
   
   ros::init(argc, argv, "ComplementaryFilter");

#if 0
   FILE *fd_batt=NULL;
   char chBattNum[50];
   fd_batt = fopen("./Batt_Cal/Batt1.txt","r");

   if(fd_batt==NULL) printf("Can't open the file\n");
   else
   {
	   fscanf(fd_batt,"%s",chBattNum);
	   printf("Found %s the file.\n",chBattNum);
   }

   unsigned int cnt_=0;
   while(fscanf(fd_batt, "%d", &batt[cnt_]) != EOF)
   {
 	  printf("%d,%d\n",batt[cnt_],cnt_);
	  cnt_++;
   }
#endif
 
	  
	   
   

   printf("*************************************\n");
   printf("*  Press space bar to take off.     *\n");
   printf("*                                   *\n");
   printf("*  Make sure extern control on and  *\n");
   printf("*  Turn on the motors               *\n");
   printf("*                                   *\n");
   printf("*************************************\n");


   printf("*************************************\n");
   printf("*  Press ESC to land                *\n");
   printf("*                                   *\n");
   printf("*************************************\n");

   #if 1
   for(int i=0;i<cnt;i++)
   {
	   ros::WallDuration(1).sleep();	 // 3 seconds delay before taking off
	   printf("%d second\n",i+1);
   }
   #endif
   
   miko::CompFilter::CompFilter compfilter;
   compfilter.last_time = ros::Time::now();
   compfilter.last_time_ = ros::Time::now();
   ros::spin();
   return 0;
}

namespace miko
{
  CompFilter::CompFilter()
  {
    ROS_INFO ("Starting complementary Filter");
    ros::NodeHandle nh;
	pub = nh.advertise<cyphy_CF::mikoCmd>("mikoCmd", 100);
	marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker",1);
	position2DSubscriber = nh.subscribe ("pose2D", 100, &CompFilter::pose2DCallback, this);
	mikoImuSubscriber = nh.subscribe ("mikoImu", 100, &CompFilter::mikoImuCallback, this);
	HeightSubscriber = nh.subscribe ("right_floor_altimeter/height", 100, &CompFilter::HeightCallback, this);
	goalSubscriber = nh.subscribe ("goal", 100, &CompFilter::goalCallback, this);
	kbdSubscriber = nh.subscribe ("keycode", 100, &CompFilter::kbdCallback, this);
	
	g_B_goal_Vel.x=0;
	g_B_goal_Vel.y=0;
	g_B_goal_Vel.z=0;

	g_GoalPosition.x=0;
	g_GoalPosition.y=0;
	//g_GoalPosition.z=0.6+HEIGHT_OFFSET; // 1m
	g_GoalPosition.z=0.6; // 1m
	g_GoalPosition.theta=0;

	g_B_esti_Vel.z=0;


	//===================================================
    //   Visualization_msg initialization
    //===================================================
    
	shape = visualization_msgs::Marker::SPHERE;
	goal_marker.header.frame_id="/world";
	goal_marker.header.stamp = ros::Time::now();
	goal_marker.ns = "basic_shapes";
	goal_marker.id = g_nShapeCnt;
	goal_marker.type = shape;
	goal_marker.action = visualization_msgs::Marker::ADD;
	goal_marker.lifetime = ros::Duration();


	goal_marker.scale.x=0.05;
	goal_marker.scale.y=0.05;
	goal_marker.scale.z=0.05;

	// Set the color -- be sure to set alpha to something non-zero!
 	goal_marker.color.r=1.0f;
	//goal_marker.color.g=1.0f;
	goal_marker.color.b=1.0f;
	goal_marker.color.a=1.0f;
	//resource=r.get("package://test.STL");
	//goal_marker.mesh_resource = "package://test.STL";

	h_pid=0;
	p_error_height_last=0;
	p_error_height=0;
	i_error_height=0;
	d_error_height=0;

	p_error_vel_last=0;
	p_error_vel=0;
	i_error_vel=0;
	d_error_vel=0;

	flag1=false;
	//take_off=false;
	uRampGas=0;
	table_cnt=0;
	take_off_table_cmd=0;
	thrust_obtain_flag=FALSE;

	landing_thrust=0;
	landing_cnt=1;


	max_batt_in_air=155;
	curr_batt=0;
	batt_cnt=1;
	start_batt_samp=0;
	batt_avg_cnt=0;
	batt_sum=0;
	h_off_ratio=0;
	max_batt=164;
	get_first_gas=0;
	prev_height_gas=0;
	
	
#if 0
	goal_marker.color.r = 0.0f;
	goal_marker.color.g = 1.0f;
	goal_marker.color.b = 0.0f;
	goal_marker.color.a = 1.0f;
#endif


	pid_yaw.initPid( 6.0 , 1.0 , 2.0 , 0.3 , -0.3 ); 
	pid_x.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
	pid_y.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
	pid_z.initPid(0.6 , 0.1 , 0.5 , 0.5 , 0 );


/*

	pid_yaw.initPid( 6.0 , 1.0 , 2.0 , 0.3 , -0.3 ); 
	pid_x.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
	pid_y.initPid( 0.4 , 0.1 , 0.5 , 0.05 , -0.05 ); 
	pid_z.initPid(0.3 , 0.05 , 0.2 , 0.3 , 0 );
*/



	pid_vel_x.initPid( 25 , 5 , 0 , 10 , -10 ); 
	pid_vel_y.initPid( 25 , 5 , 0 , 10 , -10 ); 
	pid_vel_z.initPid( 17 , 5 , 0 , 20 , 0 ); 
	


/*
	pid_vel_x.initPid( 20 , 5 , 0 , 10 , -10 ); 
	pid_vel_y.initPid( 20 , 5 , 0 , 10 , -10 ); 
	pid_vel_z.initPid( 16 , 5 , 0 , 20 , 0 ); 
*/

//	pid_height.initPid(1,1,1,130,0);

    pos_gain[P_GAIN]=0.4;
    pos_gain[I_GAIN]=0.1;
    pos_gain[D_GAIN]=0.5;
    pos_gain[I_MAX]=0.05;
    pos_gain[I_MIN]=-0.05;

	vel_gain[P_GAIN]=20;
    vel_gain[I_GAIN]=5;
    vel_gain[D_GAIN]=0;
    vel_gain[I_MAX]=10;
    vel_gain[I_MIN]=-10;

	yaw_gain[P_GAIN]=6;
    yaw_gain[I_GAIN]=1;
    yaw_gain[D_GAIN]=2;
    yaw_gain[I_MAX]=0.3;
    yaw_gain[I_MIN]=-0.3;


	height_gain[P_GAIN]=6;
    height_gain[I_GAIN]=5;
    height_gain[D_GAIN]=4;
    height_gain[I_MAX]=15;
    height_gain[I_MIN]=-15;

	vel_term=0;

#if 0
	height_new_gain[P_GAIN]=0.3;
    height_new_gain[I_GAIN]=0.05;
    height_new_gain[D_GAIN]=0.2;
    height_new_gain[I_MAX]=0.2;
    height_new_gain[I_MIN]=0;

	height_vel_gain[P_GAIN]=16;
    height_vel_gain[I_GAIN]=5;
    height_vel_gain[D_GAIN]=0;
    height_vel_gain[I_MAX]=20;
    height_vel_gain[I_MIN]=0;

	h_bound_cnt=0;
#endif	

	height_new_gain[P_GAIN]=2;
    height_new_gain[I_GAIN]=0.2;
    height_new_gain[D_GAIN]=0.3;
    height_new_gain[I_MAX]=10;
    height_new_gain[I_MIN]=-10;

	height_vel_gain[P_GAIN]=4;
    height_vel_gain[I_GAIN]=1.5;
    height_vel_gain[D_GAIN]=0;
    height_vel_gain[I_MAX]=10;
    height_vel_gain[I_MIN]=-10;


//	height_offset_init=115;
//	height_offset=115;

	quad_height=HEIGHT_OFFSET;

	h_pid_old=0;
	
	// proportional gain, integral gain, derivative gain, integral upper limit, integral lower limit

//AnglePitch,AngleRoll,AngleYaw,ACCX,ACCY,ACCZ,esti_x,esti_y
    //fd=fopen("IROS_Vel_esti.data","w");
    fd=fopen("height_all.data","w");
    //fd=fopen("yaw_cmd","w");
	//fd_=fopen("cmd.data","w");
	fd_=fopen("pose_laser.data","w");
	
    fd_h=fopen("height","w");

	// To create CSV type log file.
	fprintf(fd_,"g_CurrentPosition.x,g_CurrentPosition.y,g_CurrentPosition.theta,yaw.cmd,index\n");
//	fprintf(fd,"AnglePitch,AngleRoll,AngleYaw,AccX,AccY,AccZ,g_B_esti_Vel.x,g_B_esti_Vel.y,Index\n");
//	fprintf(fd,"g_B_Vel.x,g_B_Vel.y,g_B_esti_Vel.x,g_B_esti_Vel.y,Index\n");
//	fprintf(fd,"g_B_goal_Vel.x,g_B_goal_Vel.y,g_B_esti_Vel.x,g_B_esti_Vel.y,g_GoalPosition.x,g_GoalPosition.y,g_CurPosition.x,g_CurPosition.y,Index\n");
//	fprintf(fd,"yaw_cmd,Index\n");

//    fprintf(fd,"STICK_GAS,PRESSURE,LASER,ACCZ,index\n");
	//fprintf(fd,"Laser_Height,Throttle,PID,H_OFFSET,VEL_TERM,index\n");
	//fprintf(fd,"accZ,g_B_Vel.z,g_B_esti_Vel.z,height,index\n");
	fprintf(fd,"accZ,g_B_Vel.z,g_B_esti_Vel.z,barometer,height,index\n");
	//fprintf(fd,"g_B_ACC.z,MyAttitude.AnglePitch,g_CurPosition.z,index\n");


    //g_throttle,g_CurPosition.z,(int)MyAttitude.ACCZ,g_nCounter_h
	fprintf(fd_h,"cmd_height,height,ACCZ,Index\n");
//	fprintf(fd,"esti_x,esti_y,Index\n");

    
    //dynamic_reconfigure::Server<cyphy_CF::onlinegainConfig>::CallbackType f = boost::bind(&callback,_1, _2);
    dynamic_reconfigure::Server<cyphy_CF::onlinegainConfig>::CallbackType f = boost::bind(&CompFilter::reconfigureCB,this, _1, _2);
    srv.setCallback(f);

    timer= nh.createTimer(ros::Duration(0.050), &CompFilter::spin,this);  //50ms timer 20Hz
    ROS_INFO ("Serial setup finished");
  }

  // For autonomous taking off and landing.
  void CompFilter::spin(const ros::TimerEvent & e)
  {


	  int i=TABLE_SIZE;
#if 1
	  if(g_quad_state==ON_TAKING_OFF)
	  {
	      // Start to take off! 
	      // Get the thrust table
	      #if 0
	      if(table_cnt!=69)
		  {
//			  printf("value =%d\n",thrust[table_cnt]);
			  take_off_table_cmd=thrust[table_cnt];
			  table_cnt++;
	      }
		  #endif

//		  if(table_cnt!=60)  // 120


#if 1

          if(curr_batt)
          {
			  if(table_cnt!=(int)(55*max_batt/curr_batt))  // 114
			  {
	//			  printf("value =%d\n",thrust[table_cnt]);
				  take_off_table_cmd+=2;
		          printf("takeoff=%d\n",take_off_table_cmd);
				  table_cnt++;
		      }
			  
			  else
			  {
			      // Quad already took off!
			      // It's in the air!
			      
				  table_cnt=0;
				  g_quad_state=ON_AIR;
			  }
          }
#endif

	  }  
#endif
	  if(landing_cnt==1)
	  {
	      //printf("===========================================================test\n");
		  landing_cnt++;

		  //if ESC key is pressed.
		  if(g_quad_state==ON_LANDING)
		  {
		     printf("Here1\n");
		     //only once obtain current thrust to land.
		     if(thrust_obtain_flag==FALSE)
		     {
		         landing_thrust=mikoCmd.throttle;
				 thrust_obtain_flag=TRUE;
				 printf("Here2 thrust=%d\n",landing_thrust);
		     }

				 //After obtaining current thrust, start to decrease certain amount of steps.	 
				 if(thrust_obtain_flag==TRUE)
				 {
				     if(g_quad_state!=TOUCH_GROUND)
				     {
					     printf("Here3 thrust=%d\n",landing_thrust);
						 if(landing_thrust>=1)
						 {
						     landing_thrust -=1;
							 printf("Here4 thrust=%d\n",landing_thrust);
							 mikoCmd.throttle=landing_thrust;
						 }
						 else
						 { 
						 	landing_thrust =0;
							printf("Here5 thrust=%d\n",landing_thrust);
							mikoCmd.throttle=landing_thrust;
						 }
				     }
				 }
	      }
	  	}
	    else landing_cnt--;
  	}

  
  CompFilter::~CompFilter()
  {
    fclose(fd);
	fclose(fd_h);
	//fclose(fd_est);
    ROS_INFO ("Destroying FlightControl Interface");
  }

      void CompFilter::mikoImuCallback (const cyphy_CF::mikoImu& mikoImu)
	{

		 current_time_ = ros::Time::now();
	     //ROS_INFO ("mikoImuCallback works fine");
         MyAttitude.AnglePitch=mikoImu.anglePitch;
         MyAttitude.AngleRoll=mikoImu.angleRoll;
         MyAttitude.AngleYaw=mikoImu.angleYaw;
         MyAttitude.ACCX=mikoImu.linear_acceleration.x;
         MyAttitude.ACCY=mikoImu.linear_acceleration.y;
         MyAttitude.ACCZ=mikoImu.linear_acceleration.z;
		 g_throttle=(unsigned int)mikoImu.stick_throttle;
		 g_barome_height=mikoImu.barome_height;


		 //====================================================================
         //   Battery level check every 5 sec, this loop has 30ms sampling time
         //====================================================================


		 if( (batt_cnt%(30*1) == 0) || start_batt_samp)
		 {
		   start_batt_samp=1;
		   if(mikoImu.batt)
		   {
		       if(batt_avg_cnt==10)
		       {
			       curr_batt = batt_sum/10;
				   printf("curr_batt=%d\n",curr_batt);
				   start_batt_samp=0;
				   batt_avg_cnt=0;
				   batt_sum=0;
		       }
			   else
			   {
				   batt_sum += mikoImu.batt;
				   batt_avg_cnt++;
				   //printf("batt_sum=%d %d\n",batt_sum,batt_avg_cnt);
			   }
		   }
		   
		 }
		 batt_cnt++;


		 //===================================================
         //   Complementary Filter implementation
         //===================================================

		 
		 
//		 printf("g_B_Vel.x =%f g_B_Vel.y =%f\n",g_B_Vel.x,g_B_Vel.y);


		 //===================================================
         //  Complementary filter
         //===================================================

         g_B_ACC.x= (MyAttitude.ACCX-g*sin(MyAttitude.AnglePitch))/cos(MyAttitude.AnglePitch);

//         g_B_ACC.x = -(cos(DEG_TO_RAD(MyAttitude.AngleRoll/10.))*sin(DEG_TO_RAD(MyAttitude.AnglePitch/10.))*cos(DEG_TO_RAD(MyAttitude.AngleYaw))
//		 	+ sin(DEG_TO_RAD(MyAttitude.AngleRoll/10.))*sin(DEG_TO_RAD(MyAttitude.AngleYaw)))*((g_DebugData.Analog[THRUST]*0.2)/0.8);
//		 printf("g_B_ACC=%f\n",g_B_ACC.x);

		 
         g_B_ACC.y= (MyAttitude.ACCY+g*sin(MyAttitude.AngleRoll))/cos(MyAttitude.AngleRoll);
//         printf("g_B_ACC.x=%f\n",g_B_ACC.x);
//		 printf("g_B_ACC.y=%f\n",g_B_ACC.y);

//         g_B_ACC.z= MyAttitude.ACCZ/(cos(MyAttitude.AngleRoll)*cos(MyAttitude.AnglePitch));  // m/s^2

	     // Because downward is our positive direction. Downward is negative for MK coordinate.
         g_B_ACC.z= -MyAttitude.ACCZ; //m/s^2
         
         //g_B_ACC.z= MyAttitude.ACCZ; //m/s^2
         //g_B_ACC.z= MyAttitude.ACCZ/10;

         //printf("g_B_ACC.z = %f\n",g_B_ACC.z);

//		 printf("accZ = %f velZ = %f estiVel_Z =%f\n",g_B_ACC.z,g_B_Vel.z,g_B_esti_Vel.z);
         


//		 g_B_ACC.y= -(MyAttitude.ACCY-g*sin(DEG_TO_RAD(MyAttitude.AngleRoll)))/cos(DEG_TO_RAD(MyAttitude.AngleRoll));

//         g_B_esti_Vel.x = g_B_esti_Vel.x + 0.1*(K*(g_B_Vel.x-g_B_esti_Vel.x)+g_B_ACC.x);
//         double temp = ;

         g_B_esti_Vel.x = g_B_esti_Vel.x + (current_time_.toSec()-last_time_.toSec())*((double)(K*(g_B_Vel.x-g_B_esti_Vel.x))+(double)g_B_ACC.x);
         g_B_esti_Vel.y = g_B_esti_Vel.y + (current_time_.toSec()-last_time_.toSec())*((double)(K*(g_B_Vel.y-g_B_esti_Vel.y))+(double)g_B_ACC.y);
		 g_B_esti_Vel.z = g_B_esti_Vel.z + (current_time_.toSec()-last_time_.toSec())*((double)(0.5*(g_B_Vel.z-g_B_esti_Vel.z))+(double)g_B_ACC.z);
//		 g_B_esti_Vel.z *=-1;

		 //printf("esti_X=%f,esti_Y=%f\n",g_B_esti_Vel.x,g_B_esti_Vel.y);

		 //printf("                   esti_Z=%f\n",g_B_esti_Vel.z);



		 if(g_quad_state==ON_AIR)
		 {
		     if(current_time_.toSec()-last_time_.toSec()<0.055)
		     {
				//===================================================
				//   Velocity PI controller implementation.
				//===================================================

				//mikoCmd.pitch=Kp_vel*(g_B_goal_Vel.x-g_B_esti_Vel.x);
				pid_vel_x.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]);  //p , i , d, i_max, i_min

				//printf("esti_vel=%f goal_vel=%f\n",g_B_esti_Vel.x,g_B_goal_Vel.x);
				mikoCmd.pitch= pid_vel_x.updatePid(g_B_esti_Vel.x-g_B_goal_Vel.x,current_time_-last_time_);

				//printf("pid vel_x=%d\n",mikoCmd.pitch);

				//printf("p cmd=%d\n",mikoCmd.pitch);

				if(mikoCmd.pitch >=MAX_PITCH ) mikoCmd.pitch = MAX_PITCH;
				if(mikoCmd.pitch <=-MAX_PITCH ) mikoCmd.pitch = -MAX_PITCH;

				 //mikoCmd.roll=Kp_vel*(g_B_goal_Vel.y-g_B_esti_Vel.y);

				pid_vel_y.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]);  //p , i , d, i_max, i_min
				mikoCmd.roll= pid_vel_y.updatePid(g_B_esti_Vel.y-g_B_goal_Vel.y,current_time_-last_time_);

				//printf("pid pos_y=%f\n",mikoCmd.roll);
				//printf("r cmd=%d\n",mikoCmd.roll);

				if(mikoCmd.roll >=MAX_ROLL ) mikoCmd.roll = MAX_ROLL;
				if(mikoCmd.roll <=-MAX_ROLL ) mikoCmd.roll = -MAX_ROLL;

				pid_vel_z.setGains(height_vel_gain[P_GAIN], height_vel_gain[I_GAIN], height_vel_gain[D_GAIN], height_vel_gain[I_MAX], height_vel_gain[I_MIN]);  //p , i , d, i_max, i_min
				h_pid = pid_vel_z.updatePid(g_B_esti_Vel.z-g_B_goal_Vel.z,current_time_-last_time_);

				//h_pid=PID_vel_height(g_B_goal_Vel.z-g_B_esti_Vel.z,
				//height_vel_gain[P_GAIN],  height_vel_gain[I_GAIN],  height_vel_gain[D_GAIN],  
				//height_vel_gain[I_MAX],  height_vel_gain[I_MIN],  current_time_.toSec()-last_time_.toSec());

				//h_pid=PID_height(g_GoalPosition.z-g_CurPosition.z,
				//height_vel_gain[P_GAIN],  height_vel_gain[I_GAIN],  height_vel_gain[D_GAIN],  
				//height_vel_gain[I_MAX],  height_vel_gain[I_MIN],  current_time_.toSec()-last_time_.toSec());

				
				//printf("esti_vel_z = %f, goal_vel_z=%f\n",g_B_esti_Vel.z,g_B_goal_Vel.z);
		     }
		}

		 //printf("pid pos_y=%f\n",mikoCmd.roll);
		 

		 //if(mikoCmd.throttle >=MAX_THROTTLE ) mikoCmd.throttle = MAX_THROTTLE;
		 //if(mikoCmd.throttle <=0 ) mikoCmd.throttle = 0;

		 #if 0

		 mikoCmd.throttle=g_throttle+Kp_height_vel*(g_B_goal_Vel.z-g_B_esti_Vel.z);

		 if(mikoCmd.throttle >=MAX_THROTTLE ) mikoCmd.throttle = MAX_THROTTLE;
		 if(mikoCmd.throttle <=-MAX_THROTTLE ) mikoCmd.throttle = -MAX_THROTTLE;

		 printf("mikoCmd.throttle =%d\n",mikoCmd.throttle);

		 #endif

#if 0
		 fprintf(fd_,"%d,%d,%d\n",
			mikoCmd.pitch,    mikoCmd.roll, g_nCounter1);
	    g_nCounter1++;

		#endif
		

		 

		 


		 //===================================================
         //   Comments for the MK quadrotor.
         //===================================================
         /*
          In order to adjust Mikrokopter flight controller.
          Mikrokopter doesn't have concept of any axis. It only has pitch,roll and yaw.
		  Red rod is the front and the opposite one is the rear. Pitch(Nick) means forwards and backwrads
		  movements. In contrast, Roll means left and right movements.
	      When pushing all the way up the pitch stick(Right stick on mode2), 
	      "StickNick" value(516) is transmitted
		  to the FlightControl(FC). This implies that moving forward as fastest speed.
		  Actually it means maximum angular velocity of pitch. 
		  When pulling, opposingly all the way down the pitch stick, "StickNick" value(-592) is transmitted
		  to the FlightControl. This implies that moving backward as fastest speed.
		  On the other hand, When pushing all the way right the Roll stick, "StickRoll" value(-516) is transmitted
	      to the FC. when pushing all the way left the Roll stick, 
	      "StickRoll" value(512) is transmitted to the FC.
	     */
	     
		 mikoCmd.pitch*=1;
 		 mikoCmd.roll*=-1;
 		 mikoCmd.throttle*=1;
		 
		 //printf("mikoCmd.throttle=%d\n",mikoCmd.throttle);

		 mikoCmd.yaw=yaw_ctrl/YAW_SCALE_FACTOR;
		 //printf("mikoCmd.yaw=%d\n",mikoCmd.yaw);
		 //mikoCmd.throttle=MAX_THROTTLE;

         
	 

		 


         switch(g_quad_state)
         {
		 	case ON_AIR:
//			mikoCmd.throttle=height_offset+h_pid_old; //position PID, It works well however not stable.
			
			//printf(" height_offset = %d %f\n",(int)((float)((max_batt_in_air/curr_batt))*(float)height_offset),(float)((max_batt_in_air/curr_batt)));
#if 0
			if(curr_batt)
			{
				h_off_ratio=(float)max_batt_in_air/(float)curr_batt;
				height_offset = h_off_ratio*(float)height_offset_init;
				printf(" height_offset = %d %.4f\n",(int)height_offset,h_off_ratio);
	        }
#endif

			// Obtain the first gas only once after taking off.
            // Because take_off_table_cmd value changes based on battery level.
            // For example, if battery level is high, this value is low and vice versa.

#if 1
	        if(!get_first_gas)
	        {
		        height_offset = take_off_table_cmd;
				get_first_gas=true;
	        }
#endif

			#if 0
			else
			{
				height_offset = prev_height_gas;
			}
			#endif
			
				
	           
			//mikoCmd.throttle=height_offset+(int)h_pid_old;  // for CF velocity n position.

			mikoCmd.throttle=height_offset+(int)h_pid;
			//mikoCmd.throttle=(int)h_pid;  // for CF velocity n position.
//			mikoCmd.throttle=(int)h_pid;  // for CF velocity n position.
				
			//printf("height_offset=%d\n",height_offset);
			break;
			case ON_TAKING_OFF:
			mikoCmd.throttle=take_off_table_cmd;
			break;
			case TOUCH_GROUND:
			mikoCmd.throttle=60;
			break;
         }
			
		 #if 0
		 if(g_start==0)
		 {
			 mikoCmd.throttle=landing_thrust;
			 printf("0\n");
		 }
		 #endif
		 

		 if(mikoCmd.throttle>=130) mikoCmd.throttle=130;
		 if(mikoCmd.throttle<0) mikoCmd.throttle=0;

		 //printf("                                       mikoCmd.throttle=%d\n",mikoCmd.throttle);

		 printf("pitch=%d roll=%d gas=%d pid_h=%d height=%f\n",mikoCmd.pitch , mikoCmd.roll , mikoCmd.throttle , (int)h_pid , g_CurPosition.z);
         // publish to ROS MK serial driver.
         //fflush(stdout);

		 //printf("\rcamera x=%f,camera y=%f,camera z=%f",camera_pose.x,camera_pose.y,camera_pose.z);
		//fflush(stdout);

		 if(mikoCmd.pitch >20 || mikoCmd.roll >20)
		 {
			 printf("Something detected\n");
			 //return;
		 }
		 else pub.publish(mikoCmd);

		 //prev_height_gas = mikoCmd.throttle;

//AnglePitch,AngleRoll,AngleYaw,AccX,AccY,AccZ,index\n	
// 7 elements
#if 0
		fprintf(fd,"%f,%f,%f,%f,%f,%f,%f,%f,%d\n",
			MyAttitude.AnglePitch,    MyAttitude.AngleRoll,    MyAttitude.AngleYaw,
			MyAttitude.ACCX,    MyAttitude.ACCY,    MyAttitude.ACCZ,
			g_B_esti_Vel.x,g_B_esti_Vel.y,
			g_nCounter);
	    g_nCounter++;
#endif

#if 0
		fprintf(fd,"%f,%f,%f,%f,%d\n",
            g_B_Vel.x,g_B_Vel.y,
			g_B_esti_Vel.x,g_B_esti_Vel.y,
			g_nCounter);
	    g_nCounter++;
#endif

#if 0
		fprintf(fd,"%d,%d\n",mikoCmd.yaw,g_nCounter);
	    g_nCounter++;
#endif


#if 0
fprintf(fd,"%f,%f,%f,%f,%f,%f,%f,%f,%d\n",
            g_B_goal_Vel.x,g_B_goal_Vel.y,
			g_B_esti_Vel.x,g_B_esti_Vel.y,
			g_GoalPosition.x,g_GoalPosition.y,
			g_CurPosition.x,g_CurPosition.y,
			g_nCounter);
	    g_nCounter++;
#endif
	//printf("accZ = %f velZ = %f estiVel_Z =%f height=%f\n",g_B_ACC.z,g_B_Vel.z,g_B_esti_Vel.z,g_CurPosition.z);
	//fprintf(fd,"%f,%f,%f,%f,%d\n",g_B_ACC.z,g_B_Vel.z,g_B_esti_Vel.z,g_CurPosition.z,g_nCounter);
	//printf("g_barome_height=%d\n",g_barome_height);

	fprintf(fd,"%f,%f,%f,%d,%f,%d\n",g_B_ACC.z,g_B_Vel.z,g_B_esti_Vel.z,g_barome_height,g_CurPosition.z,g_nCounter);

	//fprintf(fd,"%f,%d,%d,%d,%f,%d\n", g_CurPosition.z , (int)mikoCmd.throttle, (int)h_pid, (int)height_offset,vel_term, g_nCounter);



#if 0
	fprintf(fd,"%d,%d,%f,%f,%d\n", 
	          g_throttle,  
	          g_barome_height , 
	          g_CurPosition.z , 
	          MyAttitude.ACCZ,
	          g_nCounter);
#endif

	//fprintf(fd,"%f,%f,%f,%d\n",g_B_ACC.z,MyAttitude.AnglePitch,g_CurPosition.z,g_nCounter);
	g_nCounter++;


		
#if 0
		fprintf(fd,"%f,%f,%d\n",g_B_esti_Vel.x,g_B_esti_Vel.y,g_nCounter);
	    g_nCounter++;
#endif



#if 0
		 ROS_INFO ("%f %f %f %f %f %f",
		 	MyAttitude.AnglePitch,
		 	MyAttitude.AngleRoll,
		 	MyAttitude.AngleYaw,
		 	MyAttitude.ACCX,
		 	MyAttitude.ACCY,
		 	MyAttitude.ACCZ);
#endif
			last_time_ = current_time_;

	}

    void CompFilter::pose2DCallback(const geometry_msgs::Pose2DConstPtr& pose2D)
    {
		 current_time = ros::Time::now();
	     //current_time_ = ros::Time::now();
	     g_CurPosition.x=pose2D->x; // m 
	     g_CurPosition.y=pose2D->y*-1;
		 g_CurPosition.theta= -(pose2D->theta); // radian

//g_CurPosition.x,g_CurPosition.y,g_CurPosition.theta,index\n	
// 4 elements

		 //===================================================
         //   Calculate velocity 
         //===================================================
         g_W_Vel.x = (g_CurPosition.x - g_PostPosition.x)/(current_time.toSec()-last_time.toSec());  // 100ms sampling time.
         g_W_Vel.y = (g_CurPosition.y - g_PostPosition.y)/(current_time.toSec()-last_time.toSec());  // 100ms sampling time.
		 
//		 printf("g_W_Vel.x =%f g_W_Vel.y =%f\n",g_W_Vel.x,g_W_Vel.y);
		 //printf("x=%.3fmm , y=%.3f , theta=%.3f\n",g_dblQuad_x,g_dblQuad_y,g_dblQuad_theta);

		 //===================================================
         //   Rotate to Body frame
         //===================================================

		 g_B_Vel.x = cos(g_CurPosition.theta)*g_W_Vel.x - sin(g_CurPosition.theta)*g_W_Vel.y;
	     g_B_Vel.y = sin(g_CurPosition.theta)*g_W_Vel.x + cos(g_CurPosition.theta)*g_W_Vel.y;


		 //===================================================
         //   Position proportianl controller implementation.
         //===================================================

		 if(g_quad_state==ON_AIR)
		 {
		    if(current_time.toSec()-last_time.toSec() < 0.13)
	        {
				//g_B_goal_Vel_.x=Kp_pos*(g_GoalPosition.x-g_CurPosition.x);
				pid_x.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]);  //p , i , d, i_max, i_min
				g_B_goal_Vel.x=pid_x.updatePid(g_CurPosition.x-g_GoalPosition.x,current_time-last_time);

				//printf("pid vel_x=%f\n",g_B_goal_Vel.x);

				if(g_B_goal_Vel.x >=MAX_X_VEL ) g_B_goal_Vel.x = MAX_X_VEL;
				if(g_B_goal_Vel.x <=-MAX_X_VEL ) g_B_goal_Vel.x = -MAX_X_VEL;

				pid_y.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]);  //p , i , d, i_max, i_min
				g_B_goal_Vel.y=pid_y.updatePid(g_CurPosition.y-g_GoalPosition.y,current_time-last_time);

				//printf("pid vel_y=%f\n",g_B_goal_Vel.y);

				if(g_B_goal_Vel.y >=MAX_Y_VEL ) g_B_goal_Vel.y = MAX_Y_VEL;
				if(g_B_goal_Vel.y <=-MAX_Y_VEL ) g_B_goal_Vel.y = -MAX_Y_VEL;

				//printf("%f %f\n",g_B_goal_Vel_.x,g_B_goal_Vel.x);

				pid_yaw.setGains(yaw_gain[P_GAIN], yaw_gain[I_GAIN], yaw_gain[D_GAIN], yaw_gain[I_MAX], yaw_gain[I_MIN]);  //p , i , d, i_max, i_min
				yaw_ctrl=pid_yaw.updatePid(RAD_TO_DEG(g_CurPosition.theta) - g_GoalPosition.theta,current_time-last_time);
		    }
		 }

		 //printf("pid yaw=%f\n",yaw_ctrl);

		 //double yaw_temp=pid_yaw.updatePid(g_DebugData.Analog[ANGLE_YAW] - DesiredPosition.yaw,current_time-last_time);
	     //g_B_goal_Vel.x=pid_x.updatePid(g_GoalPosition.x-g_CurPosition.x,current_time-last_time);
	     //g_B_goal_Vel.y=pid_y.updatePid(g_GoalPosition.y-g_CurPosition.y,current_time-last_time);

		 //printf("goal vel_x=%f goal vel_y=%f\n",g_B_goal_Vel.x,g_B_goal_Vel.y);

#if 1
		fprintf(fd_,"%f,%f,%f,%d,%d\n",
			g_CurPosition.x,    g_CurPosition.y,    g_CurPosition.theta, mikoCmd.yaw,
			g_nCounter1);

	    g_nCounter1++;
#endif

		
		 g_PostPosition = g_CurPosition;
		 last_time = current_time;
	}

	
	void CompFilter::HeightCallback(const sensor_msgs::Imu& msg)
    {
		 //double eP,eI,eD;
         //ROS_INFO("HeightCallback");

		 current_time__ = ros::Time::now();

		 g_CurPosition.z=msg.orientation.z;
		 //printf("before h=%f\n",g_CurPosition.z);
         // Compensate the height measurement using the pitch and roll angle.
		 g_CurPosition.z*=cos(MyAttitude.AnglePitch)*cos(MyAttitude.AngleRoll);

//		 printf("after h=%f\n",g_CurPosition.z);


         // Altitude PID control, fluctuated.
		 #if 1
//		 g_CurPosition.z=msg.orientation.z;

		 //printf("height =%f\n",g_CurPosition.z);

#if 0
         if(uRampGas>=100)
		 {
			 uRampGas =100;
			 h_pid=uRampGas+PID_Height(g_GoalPosition.z-g_CurPosition.z);
         }	 
		 else
		 {
			 uRampGas = uRampGas +1;
			 h_pid=uRampGas;
		 }
#endif

//         h_pid=PID_Height(g_GoalPosition.z-g_CurPosition.z);

	     #if 1
		 if(g_quad_state==ON_AIR)
		 {
			 //h_pid_old =PID_height(g_GoalPosition.z-g_CurPosition.z,height_gain[P_GAIN],height_gain[I_GAIN],height_gain[D_GAIN],\
		     //height_gain[I_MAX],height_gain[I_MIN],(current_time__.toSec()-last_time__.toSec())); //error,  p,  i,  d,  i_max,  i_min,  t
		 #endif
		 }

		 //printf("h_pid_old =%d\n",(int)h_pid_old);



		 //printf("new=%f old=%f\n",h_pid,htest);



		 //if(h_pid>=130) h_pid=130;
		 //if(h_pid<=0) h_pid=0;



		 
		 #endif
		 
		 
		 //h_pid=pid_height.updatePid(-(g_CurPosition.z-g_GoalPosition.z), current_time__-last_time__);
		 //pid_height.getCurrentPIDErrors(&eP,&eI,&eD);

		 //printf("eP=%f eI=%f eD=%f\n",eP,eI,eD);

		 //printf("height =%f cmd=%f\n",g_CurPosition.z,h_pid);
		 

		 #if 1

		 //===================================================
         //   Calculate z axis velocity 
         //===================================================
         //g_B_Vel.z = (g_CurPosition.z - g_PostPosition.z)/LASER_T;  // 100ms sampling time.
         g_B_Vel.z = (g_CurPosition.z - g_PostPosition.z)/(current_time__.toSec()-last_time__.toSec());

		 g_B_Vel.z *=-1; // reverse axis, downward is the positive direction of our quadrotor.

		 if(g_B_Vel.z >=MAX_Z_VEL ) g_B_Vel.z = MAX_Z_VEL;
		 if(g_B_Vel.z <=-MAX_Z_VEL ) g_B_Vel.z = -MAX_Z_VEL;

         //printf("g_CurPosition.z=%f g_PostPosition.z=%f\n",g_CurPosition.z,g_PostPosition.z);
		 //printf("b_vel=%f\n",g_B_Vel.z);

		 //===================================================
         //   Height Position proportianl controller implementation.
         //===================================================

		 //g_B_goal_Vel.z=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,current_time__-last_time__);
//		 if(g_quad_state==ON_AIR)

#if 1
		 if(g_quad_state==ON_AIR)
		 {
			//ros::Duration d(0.1);
//			printf("Start taking off!\n");
	        if(current_time__.toSec()-last_time__.toSec() < 0.13)
	        {
				pid_z.setGains(height_new_gain[P_GAIN], height_new_gain[I_GAIN], height_new_gain[D_GAIN], height_new_gain[I_MAX], height_new_gain[I_MIN]);  //p , i , d, i_max, i_min
				//float rospid_z_=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,current_time__-last_time__);
				//float rospid_z=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,d);
				g_B_goal_Vel.z=pid_z.updatePid(g_CurPosition.z-g_GoalPosition.z,current_time__-last_time__);

				//float z_height =PID_height(g_GoalPosition.z-g_CurPosition.z,
				//height_new_gain[P_GAIN],  height_new_gain[I_GAIN],  height_new_gain[D_GAIN],  
				//height_new_gain[I_MAX],  height_new_gain[I_MIN],  current_time__.toSec()-last_time__.toSec());

				//printf("g_B_goal_Vel.z=%f , z_height=%f\n",g_B_goal_Vel.z,z_height);

				//float duration_ = current_time__.toSec()-last_time__.toSec();
	//			printf("pid duration = %f error =%f g_B_goal_Vel.z = %f rospid_z = %f rospid_z_=%f\n",duration_,g_CurPosition.z-g_GoalPosition.z,g_B_goal_Vel.z,rospid_z,rospid_z_);
				//printf("error =%f g_B_goal_Vel.z = %f\n",g_GoalPosition.z-g_CurPosition.z,g_B_goal_Vel.z);
	        }
			
		 }
#endif

		 //printf("g_B_goal_Vel.z=%f \n",g_B_goal_Vel.z);

		 if(g_B_goal_Vel.z >=MAX_Z_VEL ) g_B_goal_Vel.z = MAX_Z_VEL;
		 if(g_B_goal_Vel.z <=-MAX_Z_VEL ) g_B_goal_Vel.z = -MAX_Z_VEL;


		 #if 0

		 g_B_goal_Vel.z=PID_Height(g_GoalPosition.z-g_CurPosition.z);

		 //g_B_goal_Vel.z=Kp_height_pos*(g_GoalPosition.z-g_CurPosition.z);
		 
		 if(g_B_goal_Vel.z >=MAX_Z_VEL ) g_B_goal_Vel.z = MAX_Z_VEL;
		 if(g_B_goal_Vel.z <=-MAX_Z_VEL ) g_B_goal_Vel.z = -MAX_Z_VEL;

		 
		 #endif
		 g_PostPosition.z = g_CurPosition.z;

		 #endif
		 

		 #if 1
		 //printf("%f,%f,%d,%d\n",h_pid,g_CurPosition.z,(int)uRampGas,g_nCounter_h);
		 //printf("            Height=%f\n",g_CurPosition.z);
		 fprintf(fd_h,"%f,%f,%d,%d\n",h_pid,g_CurPosition.z,(int)uRampGas,g_nCounter_h);
		 #endif
		 
		 g_nCounter_h++;

		 // Check whether the quad touch the ground or not.

		 if((g_quad_state==ON_LANDING)&&(g_CurPosition.z<=(HEIGHT_OFFSET+TOUCH_THRESHOLD)))
		 {
			 printf("touch the ground\n");
			 g_quad_state=TOUCH_GROUND;
		 }
		 last_time__ = current_time__;
		 
	}

	void CompFilter::goalCallback (const geometry_msgs::PoseStamped& msg)
	{
		 ROS_INFO("Goal received %f %f", msg.pose.position.x,msg.pose.position.y);
         goal_marker.pose.position.x = msg.pose.position.x;
		 goal_marker.pose.position.y = msg.pose.position.y;
		 goal_marker.pose.position.z = msg.pose.position.z;

		 g_GoalPosition.x = msg.pose.position.x;
         g_GoalPosition.y = -msg.pose.position.y;
		 goal_marker.id = g_nShapeCnt;
		 marker_pub.publish(goal_marker);		
		 g_nShapeCnt++;
		 
		 
	}

	void CompFilter::kbdCallback (const cyphy_kbd::KeyCode& msg)
	{
		 char data=msg.data;
//		 ROS_INFO("key_value=0x%02x\n",data);


		 switch(data)
		 {
		      case KEYCODE_SPACEBAR:
			  	printf("Start taking off!\n");
				g_quad_state=ON_TAKING_OFF;	
				break;
	 	      case KEYCODE_ESC:
			  	printf("Start landing!\n");
                g_quad_state=ON_LANDING;	 	        
	 	        break;

				 //  Our world coordinate
				 //  x
				 //  ^
				 //  |
				 //  |
		 		 //  |
		 		 //  -------->y

				 //  rviz world coordinate
				 //          x
				 //          ^
				 //          |
				 //          |
		 		 //          |
		 		 //  y<-------
		 		 //
		 		 //  q ---- w-----e
		 		 //  |            |
		 		 //  |            |
		 		 //  |            |
		 		 //  a------------s

	 		  case KEYCODE_a:
           
				g_GoalPosition.x=0.0;
				g_GoalPosition.y=0.0;
				break;

			  case KEYCODE_w:
                
				g_GoalPosition.x=1.0;
				g_GoalPosition.y=0.5;
				break;
			  case KEYCODE_q:
           
				g_GoalPosition.x=1.0;
				g_GoalPosition.y=0.0;
				break;
			  case KEYCODE_s:
             	g_GoalPosition.x=0.0;
				g_GoalPosition.y=1.0;
				break;
				
			  case KEYCODE_r:
             	g_GoalPosition.x=1.0;
				g_GoalPosition.y=0.5;
				break;
 			  case KEYCODE_c:
			  	
				break;

		 	}//swich
   }

	void CompFilter::reconfigureCB(cyphy_CF::onlinegainConfig &config, uint32_t level)
    {
	    pos_gain[P_GAIN]=config.Position_P;
	    pos_gain[I_GAIN]=config.Position_I;
	    pos_gain[D_GAIN]=config.Position_D;

		vel_gain[P_GAIN]=config.Velocity_P;
	    vel_gain[I_GAIN]=config.Velocity_I;
		
		yaw_gain[P_GAIN]=config.Yaw_P;
	    yaw_gain[I_GAIN]=config.Yaw_I;
	    yaw_gain[D_GAIN]=config.Yaw_D;

		//height_gain[P_GAIN]=config.Height_P;
	    //height_gain[I_GAIN]=config.Height_I;
	    //height_gain[D_GAIN]=config.Height_D;

		height_new_gain[P_GAIN]=config.Height_P;
	    height_new_gain[I_GAIN]=config.Height_I;
	    height_new_gain[D_GAIN]=config.Height_D;

		height_vel_gain[P_GAIN]=config.Height_vel_P;
	    height_vel_gain[I_GAIN]=config.Height_vel_I;
		height_vel_gain[D_GAIN]=config.Height_vel_D;
		
		
	    //height_offset = config.Height_OFFSET;

		//printf("%.2f %.2f %.2f\n",pos_gain[P_GAIN],pos_gain[I_GAIN],pos_gain[D_GAIN]);
	}

/*
	double CompFilter::PID_Height(double error)
	{
		  p_error = error;
		  //printf("error=%f\n",p_error);
		  // Calculate proportional contribution to command
		  p_term = KP_H * p_error;

		  // Calculate the integral error
//		  i_error = i_error + dt.toSec() * p_error;
 		  i_error = i_error + LASER_T * p_error;
		  //printf("i_error=%f\n",i_error);

		  //Calculate integral contribution to command
		  i_term = KI_H * i_error;
		
		  // Limit i_term so that the limit is meaningful in the output
		  if (i_term > 130)
		  {
			//printf("1\n");
		    i_term = 130;
		    i_error=i_term/KI_H;
		  }
		  else if (i_term < 0)
		  {
		  	//printf("2\n");
		    i_term = 0;
		    i_error=i_term/KI_H;
		  }

		  // Calculate the derivative error
		  //if (dt.toSec() != 0)
		  //{
//		    d_error = (p_error - p_error_last) / dt.toSec();

           // For the safety reason, ignore the first sample of the D term error.
           if(!flag1)
		   {
			   d_error=0;
			   flag1=true;
           }
		   else d_error = (p_error - p_error_last) / LASER_T;
//		  printf("p_error=%f p_error_last=%f\n",p_error,p_error_last);

		    //printf("d_error=%f\n",d_error);
		    p_error_last = p_error;
		  //}
		  // Calculate derivative contribution to command
		  d_term = KD_H * d_error;
		  double cmd = p_term + i_term + d_term;
		  //printf("cmd =%f\n",cmd);
		  return cmd;
	}
*/


      double p_error_height_last;
      double p_error_height,d_error_height,i_error_height;
		  double p_term_height, d_term_height, i_term_height;

      double p_error_vel_last;
      double p_error_vel,d_error_vel,i_error_vel;
		  double p_term_vel, d_term_vel, i_term_vel;


	double CompFilter::PID_height(double error,double p,double i,double d,double i_max,double i_min,double t)
	{
		  p_error_height = error;
		  //printf("error=%f\n",p_error);
		  // Calculate proportional contribution to command
		  p_term_height = p * p_error_height;

		  // Calculate the integral error
//		  i_error = i_error + dt.toSec() * p_error;
 		  i_error_height = i_error_height + t * p_error_height;
		  //printf("i_error=%f\n",i_error);

		  //Calculate integral contribution to command
		  i_term_height = i * i_error_height;
		
		  // Limit i_term so that the limit is meaningful in the output
		  if (i_term_height > i_max)
		  {
			//printf("1\n");
		    i_term_height = i_max;
		    i_error_height=i_term_height/i;
		  }
		  else if (i_term_height < i_min)
		  {
		  	//printf("2\n");
		    i_term_height = i_min;
		    i_error_height=i_term_height/i;
		  }

		  // Calculate the derivative error
		  //if (dt.toSec() != 0)
		  //{
//		    d_error = (p_error - p_error_last) / dt.toSec();

           // For the safety reason, ignore the first sample of the D term error.
           if(!flag1)
		   {
			   d_error_height=0;
			   flag1=true;
           }
		   else d_error_height = (p_error_height - p_error_height_last) / t;
//		  printf("p_error=%f p_error_last=%f\n",p_error,p_error_last);

		    //printf("d_error=%f\n",d_error);
		    p_error_height_last = p_error_height;
		  //}
		  // Calculate derivative contribution to command
		  d_term_height = d * d_error_height;
		  double cmd = p_term_height + i_term_height + d_term_height;
		  //printf("cmd =%f\n",cmd);
		  return cmd;
	}

	double CompFilter::PID_vel_height(double error,double p,double i,double d,double i_max,double i_min,double t)
	{
		  p_error_vel = error;
		  //printf("error=%f\n",p_error);
		  // Calculate proportional contribution to command
		  p_term_vel = p * p_error_vel;

		  // Calculate the integral error
//		  i_error = i_error + dt.toSec() * p_error;
 		  i_error_vel = i_error_vel + t * p_error_vel;
		  //printf("i_error=%f\n",i_error);

		  //Calculate integral contribution to command
		  i_term_vel = i * i_error_vel;
		
		  // Limit i_term so that the limit is meaningful in the output
		  if (i_term_vel > i_max)
		  {
			//printf("1\n");
		    i_term_vel = i_max;
		    i_error_vel=i_term_vel/i;
		  }
		  else if (i_term_vel < i_min)
		  {
		  	//printf("2\n");
		    i_term_vel = i_min;
		    i_error_vel=i_term_vel/i;
		  }

		  // Calculate the derivative error
		  //if (dt.toSec() != 0)
		  //{
//		    d_error = (p_error - p_error_last) / dt.toSec();

           // For the safety reason, ignore the first sample of the D term error.
           if(!flag1)
		   {
			   d_error_vel=0;
			   flag1=true;
           }
		   else d_error_vel = (p_error_vel - p_error_vel_last) / t;
//		  printf("p_error=%f p_error_last=%f\n",p_error,p_error_last);

		    //printf("d_error=%f\n",d_error);
		    p_error_vel_last = p_error_vel;
		  //}
		  // Calculate derivative contribution to command
		  d_term_vel = d * d_error_vel;
		  double cmd = p_term_vel + i_term_vel + d_term_vel;
		  //printf("cmd =%f\n",cmd);
		  return cmd;
	}

		
}
